/*  
 *  Nanjing JIYU Technology Co.,Ltd All rights reserved.
 *  Copyright (C) 2022 
 *  ==============================================================
 *     MoveTask.h
 *    小车运动控制
 *  --------------------------------------------------------------
 *  
 
 
 */

#include "MoveTask.h" 

PidTypeDef ANGLE_Z_PID;  // 定义一个 PID 类型的全局变量 ANGLE_Z_PID，用于 Z 轴角度控制

extern imu_t imu;  // imu 类型的变量 imu，用于获取 IMU 数据

extern uint8_t MoveMode;  // 表示当前的移动模式
extern int SetCarSpeed, SetCarAngle;  // 分别用于设定车速和车角

void MoveTask_Function(void const * argument)  // 定义一个移动任务函数
{
    float Move_A;  // 存储设定的车角
    int Move_S;  // 存储设定的车速
    float Yaw, YawError;  // 分别用于存储当前的偏航角和偏航误差
    float Error = 0;  // 用于存储误差值

    static float ANGLE_speed_pid[3] = {ANGLE_SPEED_PID_KP, ANGLE_SPEED_PID_KI, ANGLE_SPEED_PID_KD};  
    // 静态浮点型数组 ANGLE_speed_pid，用于存储 PID 控制的三个参数 KP, KI, KD
    
    portTickType tick = 0;  // 时间滴答类型的变量 tick，用于存储当前时间滴答
    PID_Init(&ANGLE_Z_PID, PID_POSITION, ANGLE_speed_pid, ANGLE_SPEED_PID_MAX_OUT, ANGLE_SPEED_PID_MAX_IOUT);  
    // 初始化 PID 控制器，传入 PID 结构体、模式、参数数组、最大输出和最大积分输出

    while(1)
    {
        tick = xTaskGetTickCount();  // 获取当前系统时间滴答
        Yaw = imu.yaw + tick / 7000;  // 计算当前偏航角（模拟误差引入）
        Move_A = SetCarAngle;  // 获取设定的车角
        Move_S = SetCarSpeed;  // 获取设定的车速
        YawError = Yaw - Move_A;  // 计算偏航误差
        if (YawError < -180) YawError += 360;  // 如果偏航误差小于 -180 度，则加 360 度
        else if (YawError > 180) YawError -= 360;  // 如果偏航误差大于 180 度，则减 360 度
        Error = LineError();  // 获取当前的线性误差
        
        switch (MoveMode)  // 根据移动模式选择执行的代码
        {
            case Stop:  // 停止模式
                MotorRightSet(0);  // 设置右电机速度为 0
                MotorLeftSet(0);  // 设置左电机速度为 0
                PID_clear(&ANGLE_Z_PID);  // 清除 PID 控制器的状态
                break;

            case OpenLoop:  // 开环模式
                MotorRightSet(Move_S + Move_A);  // 设置右电机速度为设定速度加上设定角度
                MotorLeftSet(Move_S - Move_A);  // 设置左电机速度为设定速度减去设定角度
                break;

            case Drive:  // 驾驶模式
                ANGLE_Z_PID.set = 0;  // 设定 PID 控制器的目标值为 0
                ANGLE_Z_PID.fdb = YawError;  // 将偏航误差赋给 PID 控制器的反馈值
                PID_Calc(&ANGLE_Z_PID);  // 计算 PID 输出

                // 限制 PID 输出的范围
                if (Move_S > 0)
									
                {
                    if (ANGLE_Z_PID.out > Move_S) ANGLE_Z_PID.out = Move_S;
                    if (ANGLE_Z_PID.out < -Move_S) ANGLE_Z_PID.out = -Move_S;
                }
                else
                {
                    if (ANGLE_Z_PID.out < Move_S) ANGLE_Z_PID.out = Move_S;
                    if (ANGLE_Z_PID.out > -Move_S) ANGLE_Z_PID.out = -Move_S;
                }
                MotorRightSet(Move_S + ANGLE_Z_PID.out);  // 设置右电机速度为设定速度加上 PID 输出
                MotorLeftSet(Move_S - ANGLE_Z_PID.out);  // 设置左电机速度为设定速度减去 PID 输出
                break;

            case DetectLine:   // 线路检测模式
                Error = -LineError();  // 获取并取反线性误差
                if (Move_S > 0) 
									ANGLE_Z_PID.set = Error * 3;  // 如果设定速度大于 0，设定 PID 目标值为误差的三倍
                else 
									ANGLE_Z_PID.set = -Error * 2;  // 否则，设定 PID 目标值为误差的两倍
                ANGLE_Z_PID.fdb = YawError;  // 将偏航误差赋给 PID 控制器的反馈值
                PID_Calc(&ANGLE_Z_PID);  // 计算 PID 输出

                // 限制 PID 输出的范围
                if (Move_S > 0)
                {
                    if(ANGLE_Z_PID.out > Move_S) 
											ANGLE_Z_PID.out = Move_S;
                    if(ANGLE_Z_PID.out < -Move_S) 
											ANGLE_Z_PID.out = -Move_S;
                }
                else
                {
                    if(ANGLE_Z_PID.out < Move_S) 
											ANGLE_Z_PID.out = Move_S;
                    if(ANGLE_Z_PID.out > -Move_S) 
											ANGLE_Z_PID.out = -Move_S;
                }
                MotorRightSet(Move_S + ANGLE_Z_PID.out);  // 设置右电机速度为设定速度加上 PID 输出
                MotorLeftSet(Move_S - ANGLE_Z_PID.out);  // 设置左电机速度为设定速度减去 PID 输出
                break;

            case TurnAngle:  // 转向角度模式
                ANGLE_Z_PID.set = 0;  // 设定 PID 控制器的目标值为 0
                ANGLE_Z_PID.fdb = YawError;  // 将偏航误差赋给 PID 控制器的反馈值
                PID_Calc(&ANGLE_Z_PID);  // 计算 PID 输出
                
                MotorRightSet(-ANGLE_Z_PID.out);  // 设置右电机速度为负的 PID 输出（反向）
                MotorLeftSet(ANGLE_Z_PID.out);  // 设置左电机速度为 PID 输出
                break;
        }
        vTaskDelayUntil(&tick, 10);  // 任务延迟 10 个滴答，保持循环周期
    }
}
